/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.circuitrunners.frc2011.minibot;
import edu.wpi.first.wpilibj.*;
import com.circuitrunners.frc2011.*;
/**
 *
 * @author James
 */
public class MinibotDeployment extends Components {
    private Solenoid extend, retract;
    private final int retracted = 0, inAction = 1 , extended = 2;
    private final long actionTime = 100, stillTime = 10;
    private long decay;
    private boolean extending;
    private int state = retracted;
    public void run() {
        switch(state){
            case retracted:
                if(getLeftButton(4) && getRightButton(4) && (FRCbot.getGameTime() > 120)){
                    decay = System.currentTimeMillis();
                    state = inAction;
                    extending = true;
                }
                break;
            case inAction:
                if(System.currentTimeMillis() - decay > actionTime){
                    extend.set(true);
                    retract.set(true);
                    decay = System.currentTimeMillis();
                    state = extending ? extended : retracted;
                } else {
                    extend.set(extending);
                    retract.set(!extending);
                }
                break;
            case extended:
                if(System.currentTimeMillis() - decay > stillTime){
                    decay = System.currentTimeMillis();
                    state = inAction;
                    extending = false;
                }
                break;
        }
    }
    public void init() {
        extend = new Solenoid(8,3);
        retract = new Solenoid(8,4);
    }
    public void disable() {
        extend.set(true);
        retract.set(true);
    }
}
